#pragma once

#include <pcl/common/transforms.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/region_growing.h>

#include <Plane.h>

#define MIN_POINTS_RATIO 0.1
#define MIN_PLANE_POINTS_RATIO 0.25
#define MIN_CLUSTER_RATIO 0.05  // 5%
#define MAX_CLUSTER_RATIO 0.8  // 20%
#define MIN_POINTS_IN_PLANE 150

inline float norm(PointType &p){ return std::sqrt(p.x*p.x + p.y*p.y + p.z*p.z);}

// 反复对一个点云进行RANSAC拟合提取平面
Plane extractPlane(PointCloud::Ptr cloud, float threshold = 0.01)
{
	#if 0
    PointCloud::Ptr cloud_filtered(new PointCloud);
	pcl::VoxelGrid<PointType> grid;
	grid.setLeafSize(0.1, 0.1, 0.1);
	grid.setInputCloud(cloud);
	grid.filter(*cloud_filtered);
	cloud->clear();
	pcl::copyPointCloud(*cloud_filtered, *cloud);
	cloud_filtered->clear();
	#endif

		//创建分割对象 -- 检测平面参数
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); //存储输出的模型的系数
		pcl::PointIndices::Ptr inliers(new pcl::PointIndices);				  //存储内点，使用的点
		pcl::SACSegmentation<PointType> seg;
		//可选设置
		seg.setOptimizeCoefficients(true);
		//必须设置
		seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型，检测平面
		seg.setMethodType(pcl::SAC_RANSAC);	//设置方法【聚类或随机样本一致性】
		seg.setMaxIterations(1000);
		seg.setDistanceThreshold(threshold);
		seg.setInputCloud(cloud);
		seg.segment(*inliers, *coefficients); //分割操作

		if (inliers->indices.size() == 0) // < cloud->points.size()*0.1)
		{
			PCL_ERROR(
				("Could not estimate a planar model for the given dataset: inline size = "
				+ to_string(inliers->indices.size()) +"\n").c_str() );
			return Plane();
		}
		
		Plane pl(coefficients->values, cloud);
		pl.computeNormalDirection();
		return pl;
}

void segmentCloud(PointCloud::Ptr &cloud, std::vector<PointCloud::Ptr> &clusters_cloud, int min_size, int max_size)
{
	pcl::search::Search<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::NormalEstimation<PointType, pcl::Normal> normal_estimator;
	normal_estimator.setSearchMethod(tree);
	normal_estimator.setInputCloud(cloud);
	normal_estimator.setKSearch(30);
	normal_estimator.compute(*normals);

	pcl::RegionGrowing<PointType, pcl::Normal> reg;
	reg.setMinClusterSize(min_size);
	reg.setMaxClusterSize(max_size);
	reg.setSearchMethod(tree);
	reg.setNumberOfNeighbours(30);
	reg.setInputCloud(cloud);
	reg.setInputNormals(normals);
	reg.setSmoothnessThreshold(3.0 / 180 * M_PI);
	reg.setCurvatureThreshold(1.0);

	std::vector<pcl::PointIndices> clusters;
	reg.extract(clusters);
	//std::cout << "区域生长得到:" << clusters.size() << "个区域." << std::endl;

	clusters_cloud.resize(clusters.size());

	// 为指针分配空间
	for(int i=0; i<clusters.size(); ++i)
		clusters_cloud[i] = boost::shared_ptr<PointCloud>(new PointCloud());


	pcl::ExtractIndices<PointType> extract;
	pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
	
	for(int i=0; i<clusters.size(); ++i)
	{
		extract.setInputCloud(cloud);
		*inliers = clusters[i];
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*clusters_cloud[i]);
	}
}

void segmentWall(PointCloud::Ptr &wall_cloud, std::vector<PointCloud::Ptr> &clusters_cloud)
{
	int min_size = wall_cloud->points.size() * MIN_CLUSTER_RATIO;
	int max_size = wall_cloud->points.size() * MAX_CLUSTER_RATIO;
	segmentCloud(wall_cloud, clusters_cloud, min_size, max_size);
}
